#include <stdio.h>
#include "car.h"
#include "linkkit/infra/infra_httpc.h"
#include "linkkit/infra/infra_log.h"
#include "linkkit/infra/infra_defs.h"
#include "aos/cli.h"

#define CAR_HTTP_TIMEOUT      (10 * 1000)
#define CAR_HTTP_RSP_LEN            300

#define TURN_TIME             (13)

#define FORWARD (1)
#define BACKWARD (-1)
#define REVOLVE_L (1)
#define REVOLVE_R (-1)

#define PERFECT_VAL_BASE  (85)
#define PERFECT_VAL_DEFAULT  (PERFECT_VAL_BASE +20)

#define f_b_OFFECT           (10)
#define BUFFER_VAL           (PERFECT_VAL_DEFAULT + 10)
#define BUFFER_VAL_NAG           (PERFECT_VAL_DEFAULT - 10)

#define none_data(val)    ((val == 0)?1:0)
#define invalid_data(val) (((val == 0) || (val > BUFFER_VAL) || (val < BUFFER_VAL_NAG))?1:0)
#define invalid_unno_data(val) (((val != 0) && ((val > BUFFER_VAL) || (val < BUFFER_VAL_NAG)))?1:0) 
#define invalid_max_data(f1) (invalid_data(f1) && (f1 > 150))
#define valid_data(val) (!invalid_data(val))

#define going_limit(f,b)  (valid_data(b) && valid_data(b))
#define going_turn_limit(f0, f1, b0, b1) (going_limit(f0, b0) && invalid_data(f1) && valid_data(b1))
#define turn_limit(f, b) ((invalid_max_data(f) || none_data(f)) && (invalid_max_data(b) || none_data(b)))
#define stright_limit(f) (invalid_max_data(f) || none_data(f))
#define turn_going_limit(f, b) (valid_data(f) && invalid_data(b)) 
#define search_limit(f,b) (invalid_data(f) || invalid_data(b))
#define stadby_limit(f, b) (invalid_data(f) && invalid_data(b))
#define diff_val(f1, f0)  (f1-f0)

typedef struct {
    char *payload;
    int payload_len;
} car_http_response_t;

static int car_ctrl_p = 0;
static car_status_t g_car_cul_status = START;
static car_status_t g_car_last_status = START;
static car_status_t g_car_sub_status = START;
static int16_t f_dist_l = 0;
static int16_t f_dist_det = 0;
int16_t b_dist_l = 0;
int16_t b_dist = 0;
static int straight_time = 0;  // *250ms
static int turn_time = TURN_TIME;  // *250ms
static int search_flag = 0;
static int turn_count = 0;

extern float eta_aa;
extern int eta_aa_open;

static int car_recv_callback(char *ptr, int length, int total_length, void *userdata)
{
    car_http_response_t *response = (car_http_response_t *)userdata;
    if (strlen(response->payload) + length > response->payload_len) {
        return FAIL_RETURN;
    }
    memcpy(response->payload + strlen(response->payload), ptr, length);

    return length;
}

static int move = 0, turn = 0;
int car_go(void)
{
    char http_url[128] = "http://";
    char http_url_frag[] = "192.168.1.199/rest/navigation/drive";

    int http_port = 80;

    int res = -1;
    void *http_handle = NULL;
    iotx_http_method_t http_method = IOTX_HTTP_POST;
    int http_timeout_ms = CAR_HTTP_TIMEOUT;
    car_http_response_t response; 
    char *http_header = "authorization: mrbase64 mrrest:YWRtaW4mYWRtaW4= \r\n" \
                        "Content-Type: application/json;charset=utf-8\r\n";
    int http_recv_maxlen = CAR_HTTP_RSP_LEN;
    char request_buff[CAR_HTTP_RSP_LEN] = {0};"\{\"params\":\{\"x\":0,\"y\":0,\"yaw\":0\}\}";
    char response_buff[CAR_HTTP_RSP_LEN] = {0};

    memset(&response, 0, sizeof(car_http_response_t));
    memcpy(http_url + strlen(http_url), http_url_frag, sizeof(http_url_frag));
	sprintf(request_buff, "\{\"params\":\{\"x\":%d,\"y\":0,\"yaw\":%d\}\}", move, turn);
    response.payload = response_buff;
    response.payload_len = CAR_HTTP_RSP_LEN;

    http_handle = wrapper_http_init();
    wrapper_http_setopt(http_handle, IOTX_HTTPOPT_URL, (void *)http_url);
    wrapper_http_setopt(http_handle, IOTX_HTTPOPT_PORT, (void *)&http_port);
    wrapper_http_setopt(http_handle, IOTX_HTTPOPT_METHOD, (void *)&http_method);
    wrapper_http_setopt(http_handle, IOTX_HTTPOPT_HEADER, (void *)http_header);
    //wrapper_http_setopt(http_handle, IOTX_HTTPOPT_CERT, (void *)pub_key);
    wrapper_http_setopt(http_handle, IOTX_HTTPOPT_TIMEOUT, (void *)&http_timeout_ms);
    wrapper_http_setopt(http_handle, IOTX_HTTPOPT_RECVCALLBACK, (void *)car_recv_callback);
    wrapper_http_setopt(http_handle, IOTX_HTTPOPT_RECVMAXLEN, (void *)&http_recv_maxlen);
    wrapper_http_setopt(http_handle, IOTX_HTTPOPT_RECVCONTEXT, (void *)&response);

    res = wrapper_http_perform(http_handle, request_buff, strlen(request_buff));
    wrapper_http_deinit(&http_handle);

    if (res < 0) {
        return res;
    }

#ifdef INFRA_LOG_NETWORK_PAYLOAD
    //printf("Downstream Payload:");
    //iotx_facility_json_print(response_buff, LOG_INFO_LEVEL, '<');
#endif
    //res = _preauth_parse_auth_rsp_string(response_buff, strlen(response_buff), preauth_output);

    return res;
}

int car_ctrl_change(car_status_t ctrl)
{
	g_car_last_status = g_car_cul_status;
	g_car_cul_status = ctrl;
	if (ctrl == ROUND){
		g_car_sub_status = GOING;
	}
}

car_status_t get_car_ctrl(void)
{
	return g_car_cul_status;
}


static void car_adjust(int *m, int *t, int16_t val)
{
	int temp1 = BUFFER_VAL;
	int temp2 = BUFFER_VAL_NAG;
	front(1);
	*m = 1;
	#if 0
	if (eta_aa_open){
		if (eta_aa > 1.5){
			*m = 0;
			*t = -1;
			return ;
		}else if (eta_aa > 1.3 && eta_aa < 1.5){
			*t = -1;
			return ;
		}else if (eta_aa < 1.1){
			*t = 1;
			return;
		}else{

		}
	}
	#endif
/*
	if (diff_val(f_dist, b_dist) > 60){
		*t = -1;
	} else if (diff_val(f_dist, b_dist) < -60){
		*t = 1;
	} else*/
	{
		if ((diff_val(f_dist_det, f_dist_l) > 50) || (diff_val(f_dist_det, f_dist_l) < -50)){
			return ;
		}
		if (g_car_sub_status == STRAIGHT){
			temp1 = BUFFER_VAL + 30;
			temp2 = BUFFER_VAL_NAG + 30;
		}
		if (invalid_unno_data(f_dist_det)){
			if (f_dist_det > temp1){
				*t = 1;
				right(0);
				aos_msleep(5);
				left(1);
			}
			else if (f_dist_det < temp2){
				*t = -1;
				left(0);
				aos_msleep(5);
				right(1);
			}
		} else if (diff_val(f_dist_det, f_dist_l) > val){
			*t = 1;
			right(0);
			aos_msleep(5);
			left(1);
		} else if (diff_val(f_dist_det, f_dist_l) < -val){
			*t = -1;
			left(0);
			aos_msleep(5);
			right(1);
		}else{
			*t = 0;
			left(0);
			aos_msleep(5);
			right(0);
		}	
	}
	
}

static void car_search_ctrl(int *m, int *t)
{
	if (search_flag && f_dist_det == 0){
		car_ctrl_change(STANDBY);
		search_flag = 0;
		return;
	}

	if (!search_flag){
		if (f_dist_det == 0){
			*m = 1;
			*t = 0;
		} else if (valid_data(f_dist_det) && diff_val(f_dist_det, f_dist_l) == 0 ) {
			search_flag = 1;
		} else {
			car_adjust(m, t, 5);
		}
	}else{
		*m = -1;
		*t = 0;
	}
	
}

static void car_standby_ctrl(int *m, int *t)
{
	*m = 0;
	front(0);
	*t = 0;
	left(0);
	right(0);
}

static void car_going_ctrl(int *m, int *t)
{
	static int going = 0; 
	static int flag = 0;
	if (flag == 0)
		car_adjust(m, t, 5);
	
	if (f_dist_l != 0 && f_dist_det == 0 && flag == 0){
		flag = 1;
	} 

	if (flag == 1){
		if (f_dist_det == 0){
			going++;
			if (going >= 5){
				going = 0;
				flag = 0;
				straight_time = 38;
				*m = 0;
				*t = 0;
				front(0);
				left(0);
				right(0);
				g_car_sub_status = STRAIGHT;
			}
		}else{
			flag = 0;
		}
	}
}

static void car_staight_ctrl(int *m, int *t)
{
/*
	if ((f_dist_det == 0 && f_dist_l != 0) || straight_time){
		
		if (straight_time == 0){
			straight_time = 72;
		} else if (straight_time == 1){
			straight_time = 0;
			g_car_sub_status = TURNNING;
		}else{
			behind(0);
			aos_msleep(5);
			front(1);
			aos_msleep(5);
			left(0);
			aos_msleep(5);
			right(0);
			straight_time--;
		}
				
	}else{
		car_adjust(m, t, 30);
	}
*/
	if (straight_time > 0){
		straight_time--;
		*m = 1;
		*t = 0;
		front(1);
		left(0);
		right(0);
	}else{
		front(0);
		g_car_sub_status = TURNNING;
	}
}

static void car_turnning_ctrl(int *m, int *t)
{
	if (turn_time == 0){
		if (turn_count == 3){
			turn_count=0;
			g_car_sub_status = STRAIGHT;
			car_ctrl_change(STANDBY);
		}else{
			g_car_sub_status = GOING;
		}
		turn_count++;
		if (turn_count == 2 || turn_count == 3){
			turn_time = TURN_TIME-3;
		}else{
			turn_time = TURN_TIME;
		}
		
	}else {
		left(1);
		turn_time--;
	}
}

int car_round_ctrl(int *m, int *t)
{
	switch(g_car_sub_status){
		case STRAIGHT:
			car_staight_ctrl(m , t);
			break;
		case GOING:
			car_going_ctrl(m,t);
			break;
		case TURNNING:
			car_turnning_ctrl(m, t);
			break;
		default:
			break;
	}
	printf("subs:%d\r\n", g_car_sub_status);
	return 0;
}


int car_ctrl()
{
	int *m = &move;
	int *t = &turn;
	f_dist_l = f_dist_det;
	f_dist_det = get_f_dist();
	switch(g_car_cul_status){
		case START:
			break;
		case STANDBY:
			car_standby_ctrl(m, t);
			break;
		case ROUND:
			car_round_ctrl(m , t);
			break;
		case SEARCH:
			car_search_ctrl(m, t);
			break;
		case STOP:
			*m = 0;
			*t = 0;
			front(0);
			behind(0);
			left(0);
			right(0);
			break;
		default:
			break;
	}
	if (car_ctrl_p)
		printf("culs %d las %d f_diff %d [%d %d] mt[%d %d]\r\n", 
			g_car_cul_status, g_car_last_status, f_dist_det-f_dist_l, f_dist_det, b_dist, *m, *t);
	return 0;
}

static void car_cmd_routine(char *buf, int len, int argc, char **argv)
{
	int arg1 = strtoul(argv[2], NULL, 10);
	int arg2 = strtoul(argv[3], NULL, 10);

	switch (*(argv[1]))
	{
	case 'f':
	   //aos_cli_printf("move %d\r\n", arg1);
		//move = arg1;
		front(arg1);
	   break;
	case 'b':
	   //aos_cli_printf("move %d\r\n", arg1);
		//move = arg1;
		behind(arg1);
	   break;
	case 'l':
	   //aos_cli_printf("turn %d\r\n", arg1);
		left(arg1);
	   break;
	case 'r':
	   //aos_cli_printf("turn %d\r\n", arg1);
		right(arg1);
	   break;
	case 's':
	   //aos_cli_printf("stop %d\r\n", arg1);
		stop(arg1);
	   break;
	case 'g':
	   //aos_cli_printf("stop %d\r\n", arg1);
		turn = 0;
		move = 1;
	   break;
	case 'c':
	   	move = 0;
	    turn = 0;
		car_ctrl_change(arg1);
		//g_car_sub_status = arg2;
	   break;
	case 'p':
	   	car_ctrl_p = arg1;
	   break;
	case 't':
	   	straight_time = arg1;
	   break;
	default:
	   //aos_cli_printf("car Unknown cs %d\r\n", *(argv[1]));
	   break;
	}
}

static struct cli_command car_cmd[] = {
    { "car","control or get status of car", car_cmd_routine },
};

void car_init(void)
{
	aos_cli_register_commands(&car_cmd[0], sizeof(car_cmd) / sizeof(struct cli_command));
}



